#include "SP3Interpolator.h"
#include "Utils.h"
#include <fstream>
#include <sstream>
#include <iomanip>
#include <algorithm>
#include <cmath>
#include <iostream>

SP3Interpolator::SP3Interpolator() : interpOrder_(8) {
    // 默认使用8阶拉格朗日插值
}

SP3Interpolator::~SP3Interpolator() {
    // 资源清理
}

bool SP3Interpolator::readSP3File(const std::string& filename) {
    std::ifstream file(filename);
    if (!file.is_open()) {
        std::cerr << "无法打开SP3文件: " << filename << std::endl;
        return false;
    }
    
    std::string line;
    double refTime = 0.0;  // 参考时间(GPS周内秒)
    int refWeek = 0;       // GPS周
    bool isPos = true;     // 是否为位置行(而非速度行)
    
    // 清空现有数据
    sp3Data_.clear();
    
    while (std::getline(file, line)) {
        if (line.empty()) continue;
        
        char recordType = line[0];
        
        switch (recordType) {
            case '#': {  // 历元头行
                int year, month, day, hour, minute;
                double second;
                
                if (line.length() < 60) continue;
                
                // 解析时间
                std::istringstream iss(line.substr(3));
                iss >> year >> month >> day >> hour >> minute >> second;
                
                // 转换为GPS时间
                Utils::GPSTime gpsTime = Utils::ymdhmsToGpst(year, month, day, hour, minute, second);
                refWeek = gpsTime.week;
                refTime = gpsTime.secondsOfWeek;
                
                // 重置位置/速度标志
                isPos = true;
                break;
            }
            case '*': {  // 历元行
                int year, month, day, hour, minute;
                double second;
                
                if (line.length() < 30) continue;
                
                // 解析时间
                std::istringstream iss(line.substr(3));
                iss >> year >> month >> day >> hour >> minute >> second;
                
                // 转换为GPS时间
                Utils::GPSTime gpsTime = Utils::ymdhmsToGpst(year, month, day, hour, minute, second);
                double time = gpsTime.week * 604800.0 + gpsTime.secondsOfWeek;
                
                // 更新当前历元时间
                refTime = time;
                isPos = true;  // 重置为位置行
                break;
            }
            case 'P': {  // 卫星位置行
                if (line.length() < 60) continue;
                
                // 解析卫星ID
                std::string prn = line.substr(1, 3);
                prn = Utils::trim(prn);
                
                // 解析位置(单位:km -> 转为m)
                double x = std::stod(line.substr(4, 14)) * 1000.0;
                double y = std::stod(line.substr(18, 14)) * 1000.0;
                double z = std::stod(line.substr(32, 14)) * 1000.0;
                
                // 解析钟差(单位:微秒 -> 转为秒)
                double clk = std::stod(line.substr(46, 14)) * 1.0e-6;
                
                // 创建或更新SP3数据
                if (sp3Data_.find(prn) == sp3Data_.end()) {
                    sp3Data_[prn] = std::vector<SP3Data>();
                }
                
                SP3Data data;
                data.time = refTime;
                data.pos = {x, y, z, clk};
                data.vel.clear();  // 先设为空，后续如果有速度行将填充
                
                sp3Data_[prn].push_back(data);
                break;
            }
            case 'V': {  // 卫星速度行
                if (line.length() < 60) continue;
                
                // 解析卫星ID
                std::string prn = line.substr(1, 3);
                prn = Utils::trim(prn);
                
                // 解析速度(单位:dm/s -> 转为m/s)
                double vx = std::stod(line.substr(4, 14)) * 0.1;
                double vy = std::stod(line.substr(18, 14)) * 0.1;
                double vz = std::stod(line.substr(32, 14)) * 0.1;
                
                // 解析钟漂(单位:10^-4微秒/秒 -> 转为秒/秒)
                double clkdrift = std::stod(line.substr(46, 14)) * 1.0e-10;
                
                // 更新最后添加的位置记录
                if (sp3Data_.find(prn) != sp3Data_.end() && !sp3Data_[prn].empty()) {
                    sp3Data_[prn].back().vel = {vx, vy, vz, clkdrift};
                }
                break;
            }
            // 其他类型行忽略
        }
    }
    
    file.close();
    
    // 按时间排序每颗卫星的数据
    for (auto& data : sp3Data_) {
        std::sort(data.second.begin(), data.second.end(), 
                 [](const SP3Data& a, const SP3Data& b) {
                     return a.time < b.time;
                 });
    }
    
    std::cout << "成功读取SP3文件: " << filename << std::endl;
    std::cout << "包含 " << sp3Data_.size() << " 颗卫星的精密星历" << std::endl;
    
    return !sp3Data_.empty();
}

bool SP3Interpolator::getSatellitePosVel(const std::string& prn, double time, 
                                        std::vector<double>& pos, std::vector<double>* vel) {
    // 检查是否有该卫星的数据
    if (sp3Data_.find(prn) == sp3Data_.end()) {
        return false;
    }
    
    const auto& satData = sp3Data_[prn];
    if (satData.empty()) {
        return false;
    }
    
    // 检查时间是否在数据范围内
    if (time < satData.front().time || time > satData.back().time) {
        return false;
    }
    
    // 找到最接近的历元索引
    int idx = 0;
    while (idx < satData.size() - 1 && satData[idx + 1].time <= time) {
        idx++;
    }
    
    // 确定插值区间
    int n = interpOrder_;
    if (n >= satData.size()) {
        n = satData.size() - 1;
    }
    
    int startIdx = std::max(0, idx - n/2);
    int endIdx = std::min((int)satData.size() - 1, startIdx + n);
    startIdx = std::max(0, endIdx - n);
    
    // 收集时间和位置数据
    std::vector<double> times;
    std::vector<std::vector<double>> positions;
    
    for (int i = startIdx; i <= endIdx; i++) {
        times.push_back(satData[i].time);
        positions.push_back(satData[i].pos);
    }
    
    // 位置插值
    pos.resize(4, 0.0);  // x, y, z, clk
    lagrangeInterpolation(times, positions, time, pos);
    
    // 计算速度(如果需要)
    if (vel != nullptr) {
        vel->resize(4, 0.0);  // vx, vy, vz, clkdrift
        
        // 如果SP3文件中包含速度，则插值速度
        bool hasVelocity = true;
        for (int i = startIdx; i <= endIdx; i++) {
            if (satData[i].vel.empty() || satData[i].vel.size() < 4) {
                hasVelocity = false;
                break;
            }
        }
        
        if (hasVelocity) {
            std::vector<std::vector<double>> velocities;
            for (int i = startIdx; i <= endIdx; i++) {
                velocities.push_back(satData[i].vel);
            }
            lagrangeInterpolation(times, velocities, time, *vel);
        } else {
            // 如果没有速度数据，则通过位置插值导数计算速度
            lagrangeInterpolationDerivative(times, positions, time, *vel);
        }
    }
    
    return true;
}

void SP3Interpolator::lagrangeInterpolation(const std::vector<double>& time, 
                                          const std::vector<std::vector<double>>& data,
                                          double t, std::vector<double>& result) {
    int n = time.size();
    if (n == 0 || data.empty() || data[0].empty()) {
        return;
    }
    
    int dim = data[0].size();
    result.resize(dim, 0.0);
    
    for (int i = 0; i < n; i++) {
        double li = 1.0;
        for (int j = 0; j < n; j++) {
            if (i != j) {
                li *= (t - time[j]) / (time[i] - time[j]);
            }
        }
        
        for (int k = 0; k < dim; k++) {
            result[k] += li * data[i][k];
        }
    }
}

void SP3Interpolator::lagrangeInterpolationDerivative(const std::vector<double>& time, 
                                                   const std::vector<std::vector<double>>& data,
                                                   double t, std::vector<double>& result) {
    int n = time.size();
    if (n == 0 || data.empty() || data[0].empty()) {
        return;
    }
    
    int dim = data[0].size();
    result.resize(dim, 0.0);
    
    for (int i = 0; i < n; i++) {
        double sum = 0.0;
        for (int j = 0; j < n; j++) {
            if (i != j) {
                double prod = 1.0;
                for (int k = 0; k < n; k++) {
                    if (k != i && k != j) {
                        prod *= (t - time[k]) / (time[i] - time[k]);
                    }
                }
                sum += prod / (time[i] - time[j]);
            }
        }
        
        for (int k = 0; k < dim; k++) {
            result[k] += sum * data[i][k];
        }
    }
}

// 添加获取特定卫星系统轨道数据的方法
std::vector<std::string> SP3Interpolator::getAvailableSatellites(const std::string& system) const {
    std::vector<std::string> satellites;
    
    for (const auto& pair : sp3Data_) {
        // 提取卫星系统标识 (第一个字符)
        std::string satSystem = pair.first.substr(0, 1);
        
        // 如果系统匹配或未指定系统，则添加到结果中
        if (system.empty() || satSystem == system) {
            satellites.push_back(pair.first);
        }
    }
    
    return satellites;
}

// 获取数据起止时间
std::pair<double, double> SP3Interpolator::getTimeRange() const {
    double minTime = std::numeric_limits<double>::max();
    double maxTime = std::numeric_limits<double>::lowest();
    
    for (const auto& pair : sp3Data_) {
        if (!pair.second.empty()) {
            minTime = std::min(minTime, pair.second.front().time);
            maxTime = std::max(maxTime, pair.second.back().time);
        }
    }
    
    return {minTime, maxTime};
}